package simunrobot.core;

import java.util.Vector;

import processing.core.PMatrix3D;
import processing.core.PVector;

public class Eslabon extends SItem{
	
	public Eslabon(SItem parent,Vector<String> data)
    {
    	super(parent);
    	type = Item.ESLABON;
    	ablock = false;
        dblock = false;
        thblock = false;
        alpblock = false;
        blocked = 0;
        if(parent.type == Item.ROBOT)
        {
        	((Robot)parent).addEslabon(this);
        	this.robot = parent;
        }
        if(parent.type == Item.ESLABON)
        {
        	((Eslabon)parent).insertChildren(this);
        	this.robot = ((Eslabon)parent).robot;
        	((Robot)this.robot).addEslabon(this); //<<< NO SE SI ESTO FUNCIONE SI ES LA MISMA INSTANCIA
        }
        setOriginalData(data);
    }
	public double getq()
	{
	    double parameters[]=new double[4];
	    getParametersFromMatrix(parameters,this);
	    if(!thblock)
	    {
	        if (parameters[2] == -3.14159)
	        {
	            return 3.14159;
	        }
	        return parameters[2];
	    }
	    if(!dblock)
	        return parameters[1];
	    if(!ablock)
	        return parameters[0];
	    if(!alpblock)
	        if (parameters[3] == -3.14159)
	            return 3.14159;
	        return parameters[3];
	}
	public void Update()
	{
	    /*if(_id == Globals::getInstance()->getSelectedId())
	        robot->EslabonMoving(_id);
	    if(changedbywidget)
	    {
	        changedbywidget = false;
	        GLdouble matrix[16];
	        getMatrix(matrix,this);
	        aframe->setFromMatrix(matrix);
	    }*/
	    //Verificar y Corregir Posici�n del Eslabon seg�n construcci�n del robot
	    
	    
	        //tomar parametros actuales
	        double parameters[]=new double[4];
	        getParametersFromMatrix(parameters,this);
	        ////// CIN INVERSA AQU�
	        /*if(CinInversa)
	        {
	            //robot->cininversa();
	        }*/
	        if(parameters[0]<amin)
	            parameters[0]=amin;
	        if(parameters[0]>amax)
	            parameters[0]=amax;
	        if(parameters[1]<dmin)
	            parameters[1]=dmin;
	        if(parameters[1]>dmax)
	            parameters[1]=dmax;
	        if(parameters[2]<thetamin)
	            parameters[2]=thetamin;
	        if(parameters[2]>thetamax)
	            parameters[2]=thetamax;
	        if(parameters[3]<alphamin)
	            parameters[3]=alphamin;
	        if(parameters[3]>alphamax)
	            parameters[3]=alphamax;

	        setDenavit("a",parameters[0]);
	        setDenavit("d",parameters[1]);
	        setDenavit("theta",parameters[2]);
	        setDenavit("alpha",parameters[3]);
	        
	        aframe.fromMatrix(getMatrix(this));
//	        aframe->setFromMatrix(matrix);
//	        aframe->getPosition(X,Y,Z);
	     
	}
	public void setOriginalData(Vector<String> value)
	{
	    OriginalParameters[0]=Double.valueOf(value.elementAt(0));
	    OriginalParameters[1]=Double.valueOf(value.elementAt(1));
	    OriginalParameters[2]=Double.valueOf(value.elementAt(2))*PI/180;
	    OriginalParameters[3]=Double.valueOf(value.elementAt(3))*PI/180;

	    setDenavit("a",OriginalParameters[0]);
	    setDenavit("d",OriginalParameters[1]);
	    setDenavit("theta",OriginalParameters[2]);
	    setDenavit("alpha",OriginalParameters[3]);

	    // L I M I T S \\
	    /*(AMAX)<<(AMIN)<<(DMAX)<<(DMIN)<<(THETAMAX)<<(THETAMIN)<< (ALPHAMAX)<<(ALPHAMIN);*/
	    if(value.size()>4)
	    amax=Float.valueOf(value.elementAt(4));
	    if(value.size()>5)
	    amin=Float.valueOf(value.elementAt(5));
	    if(value.size()>6)
	    dmax=Float.valueOf(value.elementAt(6));
	    if(value.size()>7)
	    dmin=Float.valueOf(value.elementAt(7));
	    if(value.size()>8)
	    thetamax=Float.valueOf(value.elementAt(8));
	    if(value.size()>9)
	    thetamin=Float.valueOf(value.elementAt(9));
	    if(value.size()>10)
	    alphamax=Float.valueOf(value.elementAt(10));
	    if(value.size()>11)
	    alphamin=Float.valueOf(value.elementAt(11));

	    if(amin == -1000)
	        amin = OriginalParameters[0];
	    if(amax == 1000)
	        amax = OriginalParameters[0];
	    if(dmin == -1000)
	        dmin = OriginalParameters[1];
	    if(dmax == 1000)
	        dmax = OriginalParameters[1];
	    if(thetamin == -1000)
	        thetamin = OriginalParameters[2];
	    else
	        thetamin=thetamin*PI/180;
	    if(thetamax == 1000)
	       thetamax = OriginalParameters[2];
	    else
	        thetamax=thetamax*PI/180;
	    if(alphamin == -1000)
	        alphamin = OriginalParameters[3];
	    else
	        alphamin=alphamin*PI/180;
	    if(alphamax == 1000)
	        alphamax = OriginalParameters[3];
	    else
	        alphamax=alphamax*PI/180;

	    if(amin == amax)
	    {
	        ablock = true;
	        blocked++;
	    }
	    if(dmin == dmax)
	    {
	        dblock = true;
	        blocked++;
	    }
	    if(thetamin == thetamax)
	    {
	        thblock = true;
	        blocked++;
	    }
	    if(alphamin == alphamax)
	    {
	        alpblock = true;
	        blocked++;
	    }
	    /*if(blocked < 3)
	    {
	        QMessageBox faltainfo;
	        faltainfo.setWindowTitle("Eslabon no definido");
	        faltainfo.setText("Eslabon "+mName+" no definido");
	        faltainfo.setInformativeText("El Eslabon tiene mas de un grado de libertad\n"
	                                        "No podra usar la cinematica directa, ni inversa, seleccione una opcion");
	        QPushButton *prismatico = faltainfo.addButton("Prismatico",QMessageBox::ActionRole);
	        QPushButton *rotacional = faltainfo.addButton("Rotacional",QMessageBox::ActionRole);
	        faltainfo.addButton(QMessageBox::Cancel);
	        faltainfo.exec();

	        if (faltainfo.clickedButton() == prismatico) {
	            amin = OriginalParameters[0];
	            amax = OriginalParameters[0];
	            thetamin = OriginalParameters[2];
	            thetamax = OriginalParameters[2];
	            alphamin = OriginalParameters[3];
	            alphamax = OriginalParameters[3];
	        } else if (faltainfo.clickedButton() == rotacional) {
	            amin = OriginalParameters[0];
	            amax = OriginalParameters[0];
	            dmin = OriginalParameters[1];
	            dmax = OriginalParameters[1];
	            alphamin = OriginalParameters[3];
	            alphamax = OriginalParameters[3];
	        }
	    }*/
	    
	    
	    aframe.fromMatrix(getMatrix(this));
	    //aframe.fromMatrix(matrix);

	    aframe.setReferenceFrame(parent().getFrame());
	    lastPos = aframe.position();
	}
	boolean ablock,dblock,thblock,alpblock;
	public SItem robot; /*!< Robot al que pertenece */
    public PVector lastPos;
    int blocked;
}
